﻿using System;
using UnityEngine;

namespace NinjaToolbox.PhysicsSystem
{
    [Serializable]
    public class BoxShape : IPhysicsShape
    {
        [HideInInspector]
        public Matrix4x4 matrix;
        public Vector3 offset;
        public Vector3 size;
        Vector3 mCacheExtents;

        public Bounds AABB
        {
            get
            {
                var lossyScale = matrix.lossyScale;
                var sizeMag = new Vector3(size.x * lossyScale.x, size.y * lossyScale.y, size.z * lossyScale.z).magnitude;
                return new Bounds(GetWorldCenter(), Vector3.one * sizeMag);
            }
        }

        public Vector3 P0 { get { return matrix.MultiplyPoint3x4(offset + new Vector3(-mCacheExtents.x, -mCacheExtents.y, -mCacheExtents.z)); } }
        public Vector3 P1 { get { return matrix.MultiplyPoint3x4(offset + new Vector3(mCacheExtents.x, -mCacheExtents.y, -mCacheExtents.z)); } }
        public Vector3 P2 { get { return matrix.MultiplyPoint3x4(offset + new Vector3(mCacheExtents.x, mCacheExtents.y, -mCacheExtents.z)); } }
        public Vector3 P3 { get { return matrix.MultiplyPoint3x4(offset + new Vector3(-mCacheExtents.x, mCacheExtents.y, -mCacheExtents.z)); } }

        public Vector3 P4 { get { return matrix.MultiplyPoint3x4(offset + new Vector3(-mCacheExtents.x, -mCacheExtents.y, mCacheExtents.z)); } }
        public Vector3 P5 { get { return matrix.MultiplyPoint3x4(offset + new Vector3(mCacheExtents.x, -mCacheExtents.y, mCacheExtents.z)); } }
        public Vector3 P6 { get { return matrix.MultiplyPoint3x4(offset + new Vector3(mCacheExtents.x, mCacheExtents.y, mCacheExtents.z)); } }
        public Vector3 P7 { get { return matrix.MultiplyPoint3x4(offset + new Vector3(-mCacheExtents.x, mCacheExtents.y, mCacheExtents.z)); } }

        public Vector3 XAxis { get { return matrix.rotation * Vector3.right; } }
        public Vector3 YAxis { get { return matrix.rotation * Vector3.up; } }
        public Vector3 ZAxis { get { return matrix.rotation * Vector3.forward; } }


        public Vector3 GetWorldCenter()
        {
            return matrix.MultiplyPoint3x4(offset);
        }

        public bool IsIntersects(IPhysicsShape other)
        {
            if (!AABB.Intersects(other.AABB)) return false;

            var sphereShape = other as SphereShape;
            if (sphereShape != null)
            {
                return PhysicsUtil.BoxVSSphere(matrix, offset, size, sphereShape.GetWorldCenter(), sphereShape.ConcertRadius);
            }

            var boxShape = other as BoxShape;
            if (boxShape != null)
            {
                mCacheExtents = size * 0.5f;
                boxShape.mCacheExtents = boxShape.size * 0.5f;

                return PhysicsUtil.BoxVSBox(P0, P1, P2, P3, P4, P5, P6, P7
                                                            , boxShape.P0, boxShape.P1, boxShape.P2, boxShape.P3, boxShape.P4, boxShape.P5, boxShape.P6, boxShape.P7
                                                            , XAxis, YAxis, ZAxis, boxShape.XAxis, boxShape.YAxis, boxShape.ZAxis);
            }

            var virtualCapsuleShape = other as VirtualCapsuleShape;
            if (virtualCapsuleShape != null)
            {
                return PhysicsUtil.BoxVSVirtualCapsule(matrix, offset, size, virtualCapsuleShape.matrix, virtualCapsuleShape.offset, virtualCapsuleShape.height, virtualCapsuleShape.radius, virtualCapsuleShape.direction, virtualCapsuleShape.virtualSegment);
            }

            return false;
        }

        public bool IsIntersects(Vector3 lineBegin, Vector3 lineEnd)
        {
            var direction = lineEnd - lineBegin;
            var distance = direction.magnitude;
            direction = direction.normalized;
            var ray = new Ray(lineBegin, direction);
            var intersectRayDistance = 0f;
            if (AABB.IntersectRay(ray, out intersectRayDistance)
                && intersectRayDistance > distance)
            {
                return false;
            }

            var local_LineBegin = matrix.inverse.MultiplyPoint3x4(lineBegin);
            var local_LineEnd = matrix.inverse.MultiplyPoint3x4(lineEnd);

            var result = Vector3.one * PhysicsUtil.INFINITY;
            var sizeHalf = size * 0.5f;
            var error = Vector2.one * 0.01f;

            var relativePosition = offset + (Vector3.forward * sizeHalf.z);
            var normal = Vector3.forward;
            var rect = new Rect(Vector2.zero, new Vector2(size.x, size.y) + error);
            var intersectPoint = GetIntersectPoint(Vector3.forward, relativePosition, local_LineBegin, local_LineEnd, rect);
            if (intersectPoint != null) return true;

            relativePosition = offset + (Vector3.back * sizeHalf.z);
            normal = Vector3.back;
            rect = new Rect(Vector2.zero, new Vector2(size.x, size.y) + error);
            intersectPoint = GetIntersectPoint(normal, relativePosition, local_LineBegin, local_LineEnd, rect);
            if (intersectPoint != null) return true;

            relativePosition = offset + (Vector3.left * sizeHalf.x);
            normal = Vector3.left;
            rect = new Rect(Vector2.zero, new Vector2(size.z, size.y) + error);
            intersectPoint = GetIntersectPoint(normal, relativePosition, local_LineBegin, local_LineEnd, rect);
            if (intersectPoint != null) return true;

            relativePosition = offset + (Vector3.right * sizeHalf.x);
            normal = Vector3.right;
            rect = new Rect(Vector2.zero, new Vector2(size.x, size.y) + error);
            intersectPoint = GetIntersectPoint(normal, relativePosition, local_LineBegin, local_LineEnd, rect);
            if (intersectPoint != null) return true;

            relativePosition = offset + (Vector3.up * sizeHalf.y);
            normal = Vector3.up;
            rect = new Rect(Vector2.zero, new Vector2(size.x, size.z) + error);
            intersectPoint = GetIntersectPoint(normal, relativePosition, local_LineBegin, local_LineEnd, rect);
            if (intersectPoint != null) return true;

            relativePosition = offset + (Vector3.down * sizeHalf.y);
            normal = Vector3.down;
            rect = new Rect(Vector2.zero, new Vector2(size.x, size.z) + error);
            intersectPoint = GetIntersectPoint(normal, relativePosition, local_LineBegin, local_LineEnd, rect);
            if (intersectPoint != null) return true;

            return false;
        }

        public bool IsContainPoint(Vector3 point)
        {
            var sizeHalf = size * 0.5f;

            var compareMassPoint = matrix.inverse.MultiplyPoint3x4(point);

            if (compareMassPoint.x <= offset.x + sizeHalf.x && compareMassPoint.x >= offset.x - sizeHalf.x)
            {
                if (compareMassPoint.y <= offset.y + sizeHalf.y && compareMassPoint.y >= offset.y - sizeHalf.y)
                {
                    if (compareMassPoint.z <= offset.z + sizeHalf.z && compareMassPoint.z >= offset.z - sizeHalf.z)
                    {
                        return true;
                    }
                }
            }

            return false;
        }

        public Vector3 MassPointToEdge(Vector3 massPoint, Vector3 outsidePoint)
        {
            var local_MassPoint = matrix.inverse.MultiplyPoint3x4(massPoint);
            var local_OutsidePoint = matrix.inverse.MultiplyPoint3x4(outsidePoint);

            var result = Vector3.one * PhysicsUtil.INFINITY;
            var sizeHalf = size * 0.5f;
            var error = Vector2.one * 0.01f;
            var hasValue = false;

            var relativePosition = offset + (Vector3.forward * sizeHalf.z);
            var normal = Vector3.forward;
            var rect = new Rect(Vector2.zero, new Vector2(size.x, size.y) + error);
            var intersectPoint = GetIntersectPoint(Vector3.forward, relativePosition, local_MassPoint, local_OutsidePoint, rect);
            if (intersectPoint != null) result = intersectPoint.Value; hasValue = true;
            //-----------Forward.

            relativePosition = offset + (Vector3.back * sizeHalf.z);
            normal = Vector3.back;
            rect = new Rect(Vector2.zero, new Vector2(size.x, size.y) + error);
            intersectPoint = GetIntersectPoint(normal, relativePosition, local_MassPoint, local_OutsidePoint, rect);
            if (intersectPoint != null && intersectPoint.Value.sqrMagnitude < result.sqrMagnitude) result = intersectPoint.Value; hasValue = true;
            //-----------Back.

            relativePosition = offset + (Vector3.left * sizeHalf.x);
            normal = Vector3.left;
            rect = new Rect(Vector2.zero, new Vector2(size.z, size.y) + error);
            intersectPoint = GetIntersectPoint(normal, relativePosition, local_MassPoint, local_OutsidePoint, rect);

            if (intersectPoint != null && intersectPoint.Value.sqrMagnitude < result.sqrMagnitude) result = intersectPoint.Value; hasValue = true;
            //-----------Left.

            relativePosition = offset + (Vector3.right * sizeHalf.x);
            normal = Vector3.right;
            rect = new Rect(Vector2.zero, new Vector2(size.x, size.y) + error);
            intersectPoint = GetIntersectPoint(normal, relativePosition, local_MassPoint, local_OutsidePoint, rect);
            if (intersectPoint != null && intersectPoint.Value.sqrMagnitude < result.sqrMagnitude) result = intersectPoint.Value; hasValue = true;
            //-----------Right.

            relativePosition = offset + (Vector3.up * sizeHalf.y);
            normal = Vector3.up;
            rect = new Rect(Vector2.zero, new Vector2(size.x, size.z) + error);
            intersectPoint = GetIntersectPoint(normal, relativePosition, local_MassPoint, local_OutsidePoint, rect);
            if (intersectPoint != null && intersectPoint.Value.sqrMagnitude < result.sqrMagnitude) result = intersectPoint.Value; hasValue = true;
            //-----------Up.

            relativePosition = offset + (Vector3.down * sizeHalf.y);
            normal = Vector3.down;
            rect = new Rect(Vector2.zero, new Vector2(size.x, size.z) + error);
            intersectPoint = GetIntersectPoint(normal, relativePosition, local_MassPoint, local_OutsidePoint, rect);
            if (intersectPoint != null && intersectPoint.Value.sqrMagnitude < result.sqrMagnitude) result = intersectPoint.Value; hasValue = true;
            //-----------Down.

            if (hasValue) return matrix.MultiplyPoint3x4(result);
            else throw new System.Exception("质点在Box外!");
        }

        public Vector3 GetEdgeNormal(Vector3 massPoint, Vector3 outsidePoint)
        {
            var local_MassPoint = matrix.inverse.MultiplyPoint3x4(massPoint);
            var local_OutsidePoint = matrix.inverse.MultiplyPoint3x4(outsidePoint);

            var result = Vector3.one * PhysicsUtil.INFINITY;
            var closestIntersectPoint = Vector3.one * PhysicsUtil.INFINITY;
            var sizeHalf = size * 0.5f;
            var error = Vector2.one * 0.1f;
            var hasValue = false;

            var relativePosition = offset + (Vector3.forward * sizeHalf.z);
            var normal = Vector3.forward;
            var rect = new Rect(Vector2.zero, new Vector2(size.x, size.y) + error);
            var intersectPoint = GetIntersectPoint(normal, relativePosition, local_MassPoint, local_OutsidePoint, rect);
            if (intersectPoint != null)
            {
                result = normal;
                closestIntersectPoint = intersectPoint.Value;
                hasValue = true;
            }
            //-----------Forward.

            relativePosition = offset + (Vector3.back * sizeHalf.z);
            normal = Vector3.back;
            rect = new Rect(Vector2.zero, new Vector2(size.x, size.y) + error);
            intersectPoint = GetIntersectPoint(normal, relativePosition, local_MassPoint, local_OutsidePoint, rect);
            if (intersectPoint != null && intersectPoint.Value.sqrMagnitude < closestIntersectPoint.sqrMagnitude)
            {
                result = normal;
                closestIntersectPoint = intersectPoint.Value;
                hasValue = true;
            }
            //-----------Back.

            relativePosition = offset + (Vector3.left * sizeHalf.x);
            normal = Vector3.left;
            rect = new Rect(Vector2.zero, new Vector2(size.z, size.y) + error);
            intersectPoint = GetIntersectPoint(normal, relativePosition, local_MassPoint, local_OutsidePoint, rect);

            if (intersectPoint != null && intersectPoint.Value.sqrMagnitude < closestIntersectPoint.sqrMagnitude)
            {
                result = normal;
                closestIntersectPoint = intersectPoint.Value;
                hasValue = true;
            }
            //-----------Left.

            relativePosition = offset + (Vector3.right * sizeHalf.x);
            normal = Vector3.right;
            rect = new Rect(Vector2.zero, new Vector2(size.x, size.y) + error);
            intersectPoint = GetIntersectPoint(normal, relativePosition, local_MassPoint, local_OutsidePoint, rect);
            if (intersectPoint != null && intersectPoint.Value.sqrMagnitude < closestIntersectPoint.sqrMagnitude)
            {
                result = normal;
                closestIntersectPoint = intersectPoint.Value;
                hasValue = true;
            }
            //-----------Right.

            relativePosition = offset + (Vector3.up * sizeHalf.y);
            normal = Vector3.up;
            rect = new Rect(Vector2.zero, new Vector2(size.x, size.z) + error);
            intersectPoint = GetIntersectPoint(normal, relativePosition, local_MassPoint, local_OutsidePoint, rect);
            if (intersectPoint != null && intersectPoint.Value.sqrMagnitude < closestIntersectPoint.sqrMagnitude)
            {
                result = normal;
                closestIntersectPoint = intersectPoint.Value;
                hasValue = true;
            }
            //-----------Up.

            relativePosition = offset + (Vector3.down * sizeHalf.y);
            normal = Vector3.down;
            rect = new Rect(Vector2.zero, new Vector2(size.x, size.z) + error);
            intersectPoint = GetIntersectPoint(normal, relativePosition, local_MassPoint, local_OutsidePoint, rect);
            if (intersectPoint != null && intersectPoint.Value.sqrMagnitude < closestIntersectPoint.sqrMagnitude)
            {
                result = normal;
                closestIntersectPoint = intersectPoint.Value;
                hasValue = true;
            }
            //-----------Down.

            if (hasValue) return matrix.rotation * result;
            else throw new System.Exception("质点在Box外!");
        }

        public void DrawGizmos(Color color)
        {
            var cacheMatrix = Gizmos.matrix;
            var cacheColor = Gizmos.color;

            Gizmos.matrix = matrix;

            Gizmos.color = color;
            Gizmos.DrawWireCube(offset, size);
            Gizmos.matrix = cacheMatrix;
            Gizmos.color = cacheColor;
        }

        Vector3? GetIntersectPoint(Vector3 planeNormal, Vector3 planePosition, Vector3 p0, Vector3 p1, Rect rect)
        {
            var sign1 = Mathf.Sign(Vector3.Dot(planeNormal, planePosition - p0));
            var sign2 = Mathf.Sign(Vector3.Dot(planeNormal, planePosition - p1));
            if (Mathf.Approximately(sign1, sign2)) return null;//同侧异侧.

            var a = planeNormal.x;
            var b = planeNormal.y;
            var c = planeNormal.z;
            var d = -a * planePosition.x - b * planePosition.y - c * planePosition.z;

            var i0 = a * p0.x + b * p0.y + c * p0.z;
            var i1 = a * p1.x + b * p1.y + c * p1.z;
            var final_t = -(i1 + d) / (i0 - i1);

            var finalPoint = new Vector3()
            {
                x = p0.x * final_t + p1.x * (1 - final_t),
                y = p0.y * final_t + p1.y * (1 - final_t),
                z = p0.z * final_t + p1.z * (1 - final_t),
            };

            var matrix = Matrix4x4.TRS(planePosition, Quaternion.FromToRotation(planeNormal, Vector3.forward), Vector3.one);
            var localPosition = matrix.inverse.MultiplyPoint3x4(finalPoint);

            if (localPosition.x <= rect.size.x * 0.5f
                 && localPosition.x >= -rect.size.x * 0.5f
                 && localPosition.y <= rect.size.y * 0.5f
                 && localPosition.y >= -rect.size.y * 0.5f)
            {
                return finalPoint;
            }
            else
            {
                return null;
            }
        }

        Matrix4x4 IPhysicsShape.Matrix { get { return matrix; } set { matrix = value; } }
    }
}
